﻿/**
 * A*寻路找最短路径
 */
 
using System;
using System.Collections.Generic;
using UnityEditor;
using UnityEngine;
using UnityEngine.SceneManagement;

public class AStar : MonoBehaviour
{
    private List<Triangle> mTriangleList;

    public static AStar Instance;
    void Awake()
    {
        Instance = this;
        mTriangleList = AssetDatabase.LoadAssetAtPath<GuideMeshData>("Assets/Config/" + SceneManager.GetActiveScene().name + ".asset").mTriangleList;
    }

    void OnDrawGizmos()
    {
        if (mTriangleList == null)
            return;

        for (int i = 0; i < mTriangleList.Count; i++)
        {
            Triangle triangle = mTriangleList[i];
            for (int j = 0; j < triangle.mPoints.Length; j++)
            {
                Handles.DrawLine(triangle.mPoints[j], triangle.mPoints[(j + 1) % triangle.mPoints.Length]);
            }
            Handles.Label(triangle.Center, triangle.Index.ToString());
        }
    }

    //开启列表
    List<Point> Open_List = new List<Point>();

    //关闭列表
    List<Point> Close_List = new List<Point>();

    //从开启列表查找F值最小的节点
    private Point GetMinFFromOpenList()
    {
        Point Pmin = null;
        foreach (Point p in Open_List)
            if (Pmin == null || Pmin.G + Pmin.H > p.G + p.H)
                Pmin = p;
        return Pmin;
    }

    //判断关闭列表是否包含一个坐标的点
    private bool IsInCloseList(Triangle tTriangle)
    {
        foreach (Point p in Close_List)
            if (p.mTriangle == tTriangle)
                return true;
        return false;
    }
    //从关闭列表返回对应坐标的点
    private Point GetPointFromCloseList(Triangle tTriangle)
    {
        foreach (Point p in Close_List)
            if (p.mTriangle == tTriangle)
                return p;
        return null;
    }

    //判断开启列表是否包含一个坐标的点
    private bool IsInOpenList(Triangle tTriangle)
    {
        foreach (Point p in Open_List)
            if (p.mTriangle == tTriangle)
                return true;
        return false;
    }
    //从开启列表返回对应坐标的点
    private Point GetPointFromOpenList(Triangle tTriangle)
    {
        foreach (Point p in Open_List)
            if (p.mTriangle == tTriangle)
                return p;
        return null;
    }


    //计算某个点的G值
    private float GetG(Point p)
    {
        if (p.mFather == null)
            return 0;
        else
            return p.mFather.G + Vector3.Distance(p.mFather.mTriangle.Center, p.mTriangle.Center);
    }

    //计算某个点的H值
    private float GetH(Point tCurP, Point tEndP)
    {
        return GetLenght(tCurP, tEndP);
    }

    //计算两个点的距离
    private float GetLenght(Point tP1, Point tP2)
    {
        return Vector3.Distance(tP1.mTriangle.Center, tP2.mTriangle.Center);
    }

    //检查当前节点附近的节点
    private void CheckP8(Point tStartP, ref Point tEndP)
    {
        Triangle triangle1 = tStartP.mTriangle;

        List<Triangle> temptrianglelist = new List<Triangle>();

        for (int i = 0; i < triangle1.mLines.Length; i++)
        {
            int index = triangle1.mLines[i].mLeftTriangle;
            if (index != -1)
                temptrianglelist.Add(mTriangleList[index]);
        }

        for (int i = 0; i < temptrianglelist.Count; i++)
        {
            Triangle temp = temptrianglelist[i];
            if (!IsInCloseList(temp))
            {
                if (IsInOpenList(temp))
                {
                    Point pt = GetPointFromOpenList(temp);
                    float G_new = tStartP.G + GetLenght(tStartP, pt);
                    if (G_new < pt.G)
                    {
                        Open_List.Remove(pt);
                        pt.mFather = tStartP;
                        pt.G = G_new;
                        Open_List.Add(pt);
                    }
                }
                else
                {
                    //不在开启列表中
                    Point pt = new Point(temp);
                    pt.mFather = tStartP;
                    float G_new = tStartP.G + GetLenght(tStartP, pt);
                    pt.G = G_new;
                    pt.H = GetH(pt, tEndP);
                    Open_List.Add(pt);
                }
            }
        }
    }

    private List<int> FindWay(Point tStartP, Point tEndP)
    {
        Open_List.Clear();
        Close_List.Clear();

        Open_List.Add(tStartP);
        while (!(IsInOpenList(tEndP.mTriangle) || Open_List.Count == 0))
        {
            Point p0 = GetMinFFromOpenList();
            if (p0 == null) return null;
            Open_List.Remove(p0);
            Close_List.Add(p0);
            CheckP8(p0, ref tEndP);
        }
        Point p = GetPointFromOpenList(tEndP.mTriangle);
        List<int> pathlist = new List<int>();
        while (p.mFather != null)
        {
            pathlist.Add(p.mTriangle.Index);
            p = p.mFather;
        }
        pathlist.Add(p.mTriangle.Index);

        for (int i = 0; i < pathlist.Count / 2; i++)
        {
            int change1 = i;
            int change2 = pathlist.Count - 1 - i;
            int temp = pathlist[change1];
            pathlist[change1] = pathlist[change2];
            pathlist[change2] = temp;
        }
        return pathlist;
    }

    public List<Vector3> FindWay(Vector3 tStartPos, Vector3 tEndPos)
    {
        Triangle tStartTriangle = null;
        Triangle tEndTriangle = null;
        List<Triangle> trianglelist = mTriangleList;
        for (int i = 0; i < trianglelist.Count; i++)
        {
            if (trianglelist[i].ContainPoint(tStartPos))
            {
                tStartTriangle = trianglelist[i];
            }
            if (trianglelist[i].ContainPoint(tEndPos))
            {
                tEndTriangle = trianglelist[i];
            }
        }
        if (tStartTriangle != null && tEndTriangle != null)
        {
            Point Point1 = new Point(tStartTriangle);
            Point Point2 = new Point(tEndTriangle);
            Point1.G = 0;
            Point1.H = GetH(Point1, Point2);
            List<int> pathlist = FindWay(Point1, Point2);
            return SearchInflexion(tStartPos, tEndPos, pathlist);
        }
        return null;
    }

    /// <summary>
    /// 生成拐点
    /// </summary>
    /// <param name="tStartPos"></param>
    /// <param name="tEndPos"></param>
    /// <param name="tPathStack"></param>
    /// <returns></returns>
    private List<Vector3> SearchInflexion(Vector3 tStartPos, Vector3 tEndPos, List<int> tPathList)
    {
        //不在寻路范围内
        if (tPathList.Count == 0)
            return new List<Vector3>();

        string path = "";
        for (int i = 0; i < tPathList.Count; i++)
        {
            path += tPathList[i] + "->";
        }
        Debug.Log(path);

        //在一个三角形内
        if (tPathList[0] == tPathList[tPathList.Count - 1])
            return new List<Vector3>() { tStartPos, tEndPos };

        //优化路径
        List<Vector3> vec3List = new List<Vector3>();
        vec3List.Add(tStartPos);

        Line leftLine = null;
        Line rightLine = null;
        List<Line> lines = new List<Line>();

        for (int i = 0; i < tPathList.Count - 1; i++)
        {
            Triangle triangle = mTriangleList[tPathList[i]];
            for (int j = 0; j < triangle.mLines.Length; j++)
            {
                Line line = triangle.mLines[j];
                if (line.mLeftTriangle == tPathList[i + 1])
                {
                    //生成左右线，
                    if (leftLine == null && rightLine == null)
                    {
                        Vector3 lasePoint = vec3List[vec3List.Count - 1];
                        //如果末尾点不是重叠线上的点，直接生成左右线
                        if (line.mPoint1 != lasePoint && line.mPoint2 != lasePoint)
                        {
                            leftLine = new Line(lasePoint, line.mPoint1);
                            rightLine = new Line(lasePoint, line.mPoint2);
                            break;
                        }
                    }
                    else
                    {
                        Vector3 rootPoint = leftLine.mPoint1;
                        //生成左右线向量
                        Vector3 vecleft = leftLine.mPoint2 - rootPoint;
                        Vector3 vecright = rightLine.mPoint2 - rootPoint;

                        //找出远望点
                        Vector3 thirdPoint = Vector3.zero;
                        if (leftLine.mPoint2 == line.mPoint1)
                            thirdPoint = line.mPoint2;
                        else if (rightLine.mPoint2 == line.mPoint2)
                            thirdPoint = line.mPoint1;
                        //生成远望线向量
                        Vector3 lookingLine = thirdPoint - rootPoint;

                        //远望
                        if (leftLine.mPoint2 == line.mPoint1)
                        {
                            //如果远望线在左右线中间，远望成功，收缩右线
                            if (Vector3.Cross(vecleft, lookingLine).y * Vector3.Cross(lookingLine, vecright).y >= 0)
                            {
                                rightLine.mPoint2 = thirdPoint;
                                lines.Add(line);
                                break;
                            }
                        }
                        else if (rightLine.mPoint2 == line.mPoint2)
                        {
                            //如果远望线在左右线中间，远望成功，收缩左线
                            if (Vector3.Cross(vecleft, lookingLine).y * Vector3.Cross(lookingLine, vecright).y >= 0)
                            {
                                leftLine.mPoint2 = thirdPoint;
                                lines.Add(line);
                                break;
                            }
                        }

                        //远望失败，添加拐点
                        Vector3 middlevec = Vector3.zero;
                        //远望线在右线右边，
                        if (Vector3.Cross(vecleft, vecright).y * Vector3.Cross(vecright, lookingLine).y > 0)
                            middlevec = rightLine.mPoint2;
                        //远望线在左线左边，
                        else if (Vector3.Cross(vecright, vecleft).y * Vector3.Cross(vecleft, lookingLine).y > 0)
                            middlevec = leftLine.mPoint2;

                        vec3List.Add(middlevec);
                        lines.Clear();

                        //如果拐点不是重叠线上的点，直接生成左右线
                        if (middlevec != line.mPoint1 && middlevec != line.mPoint2)
                        {
                            leftLine = new Line(middlevec, line.mPoint1);
                            rightLine = new Line(middlevec, line.mPoint2);
                        }
                        else
                        {
                            leftLine = null;
                            rightLine = null;
                        }
                        break;
                    }
                }
            }
        }
        if (leftLine != null && rightLine != null)
        {
            Vector3 originPoint = leftLine.mPoint1;
            Vector3 vecleft = leftLine.mPoint2 - originPoint;
            Vector3 vecright = rightLine.mPoint2 - originPoint;
            Vector3 middle = tEndPos - originPoint;

            if (Vector3.Cross(vecleft, middle).y * Vector3.Cross(middle, vecright).y < 0)
            {
                float leftlength = Vector3.Distance(originPoint, leftLine.mPoint2) + Vector3.Distance(leftLine.mPoint2, tEndPos);
                float rightlength = Vector3.Distance(originPoint, rightLine.mPoint2) + Vector3.Distance(rightLine.mPoint2, tEndPos);
                if (leftlength < rightlength)
                {
                    vec3List.Add(leftLine.mPoint2);
                    lines.Clear();
                }
                else
                {
                    vec3List.Add(rightLine.mPoint2);
                    lines.Clear();
                }
            }
        }

        vec3List.Add(tEndPos);

        if (lines.Count > 0)
        {
            Vector3 startPos = vec3List[vec3List.Count - 2];
            Vector3 endPos = vec3List[vec3List.Count - 1];
            float k1 = (endPos.z - startPos.z) / (endPos.x - startPos.x);
            float d1 = startPos.z - k1 * startPos.x;
            for (int i = 0; i < lines.Count; i++)
            {
                Line line = lines[i];
                Vector3 startPos2 = line.mPoint1;
                Vector3 endPos2 = line.mPoint2;
                float k2 = (endPos2.z - startPos2.z) / (endPos2.x - startPos2.x);
                float d2 = startPos2.z - k2 * startPos2.x;
                float x = (d2 - d1) / (k1 - k2);
                float t = (x - startPos2.x) / (endPos2.x - startPos2.x);
                vec3List.Insert(vec3List.Count - 1, Vector3.Lerp(startPos2, endPos2, t));
            }
        }

        return vec3List;
    }
}

class Point
{
    public Triangle mTriangle;
    public float G;
    public float H;
    public Point(Triangle tTriangle)
    {
        mTriangle = tTriangle;
    }
    public Point mFather;
}

